#include <stdio.h>
#ifndef PHIDGET_ALREADY
#define PHIDGET_ALREADY
#include "phidget21.h"
#endif
#include "robotcontroller.h"


int main(int argc, char* argv[])
{
	RobotController robo;
	
	cerr<< "Engage. (press Enter to continue)" << endl;
	getchar(); // pause until Enter
	robo.elbow.engage();
	robo.rack.engage();
	robo.gripper.engage();
	robo.shoulder.engage();

	cerr << "Move elbow by 10 degrees. (Clockwise or counterclockwise?)" << endl;
	getchar(); // pause until Enter
	double current = robo.elbow.getCapstanPositionByDegrees();
	robo.elbow.setCapstanPositionByDegrees(current + 10);

	cerr << "Open gripper by 45 degrees." << endl;
	getchar(); // pause until Enter
	robo.gripper.openByDegrees(45.0);

	cerr << "Increment rack by 20mm. (Is this up or down?)" << endl;
	getchar(); // pause until Enter
	robo.rack.incrementByMm(20.0);

	cerr << "Disengage." << endl;
	getchar(); // pause until Enter
	robo.elbow.disengage();
	robo.rack.disengage();
	robo.gripper.disengage();
	robo.shoulder.disengage();

	cerr << "Press any key to end" << endl;
	getchar(); // pause until Enter
	
	return 0;
}
